# -*- encoding: utf-8 -*-
"""
@File    :   ImagProcess.py    

@Contact :   2055466817@qq.com

@Modify Time :   2020/12/2 上午6:19 

@Author :   赵方国        
"""
import random

import cv2 as cv
import numpy as np

from naoqi import ALProxy
import vision_definitions


def ImagProgressHSV(filename, flag):
    """
        处理图片，进行阈值化。

        :param filename: 图片一张，由getImag函数获得
        :param flag: 表示对哪一种色彩进行操作，应传入字符串
        :return: 返回障碍物矩形化的x，y，w，h
    """
    Image = filename

    Image_Gau = cv.GaussianBlur(Image, (9, 9), 0)
    Image_HSV = cv.cvtColor(Image_Gau, cv.COLOR_BGR2HSV)
    # print Image_HSV[180, 180]
    # HSV阈值设定
    # lowarrayball = np.array([150, 43, 46])
    # higharrayball = np.array([180, 255, 255])  # 此处使用反向思维，直接允许红色之外的所有值通过，红色部分为黑色，在识别下摄像头时效果不错
    if flag == "blue":
        lowarray = np.array([95, 80, 75])
        higharray = np.array([115, 255, 255])  # 青色+蓝色 晚上低H90,43,46
    elif flag == "red":
        lowarray = np.array([120, 43, 46])
        higharray = np.array([180, 255, 255])  # 红色
    elif flag == "yellow":
        lowarray = np.array([15, 43, 46])
        higharray = np.array([40, 255, 255])  # 黄色阈值
    elif flag == "white":
        lowarray = np.array([0, 0, 221])
        higharray = np.array([180, 30, 255])  # 白色阈值
    # lowarraydark = np.array([0, 43, 46])
    # higharraydark = np.array([10, 255, 255])
    # 　二值化
    # dstball = cv.inRange(Image_HSV, lowarrayball, higharrayball)
    dsthole = cv.inRange(Image_HSV, lowarray, higharray)
    # dstdark = cv.inRange(Image_HSV, lowarraydark, higharraydark)
    # 中值滤波降噪，开运算降噪
    # MedirImagball = cv.medianBlur(dstball, 3)
    MedirImaghole = cv.medianBlur(dsthole, 3)
    element = cv.getStructuringElement(cv.MORPH_RECT, (3, 3))
    # MedirImagball = cv.morphologyEx(MedirImagball, cv.MORPH_OPEN, element)
    MedirImaghole = cv.morphologyEx(MedirImaghole, cv.MORPH_OPEN, element)
    # cv.imshow('mediaball', MedirImagball)
    cv.imshow('mediahole', MedirImaghole)
    # resultball = cv.Canny(MedirImagball, 50, 150)
    resulthole = cv.Canny(MedirImaghole, 50, 150)
    # cv.imshow('canny', result)
    # MedirImagdark = cv.medianBlur(dstdark, 9)
    # element = cv.getStructuringElement(cv.MORPH_RECT, (13, 13))
    # MedirImagdark = cv.morphologyEx(MedirImagdark, cv.MORPH_OPEN, element)
    # cv.imshow('testdark', MedirImag)
    # dark = cv.addWeighted(MedirImagdark, 0.5, MedirImag, 0.5, 0)
    # cv.imshow('add', dark)
    # 霍夫变换检测圆心cv.waitKey(0)
    # if flag == 'ball':
    #     circles = cv.HoughCircles(result, cv.HOUGH_GRADIENT, 1, 60, param1=1, param2=5, minRadius=1, maxRadius=20)
    #     # 参数4是圆心距离，param2是v2.HOUGH_GRADIENT方法的累加器阈值。阈值越小，检测到的圈子越多。
    #     # print circles
    #     i = 0
    #     for ci in circles[0]:
    #         i += 1
    #     if i != 1:
    #         for circle in circles[0]:
    #             if judgecircle(Image, circle):
    #                 circleflag = circle
    #     else:
    #         circleflag = circles[0]
    #     print circleflag
    #     x = int(circleflag[0][0])
    #     y = int(circleflag[0][1])
    #     r = int(circleflag[0][2])
    #     result = cv.circle(result, (x, y), r, (255, 255, 255), 1)
    #     result = cv.circle(result, (x, y), 2, (255, 255, 255), -1)
    #     data = [x, y]
    # if flag == 'hole':
    #     x, y, w, h = cv.boundingRect(result)
    #     cv.rectangle(result, (x, y), (x + w, y + h), (255, 255, 255), 2)
    #     data = [x + w / 2, y + h]
    # # print data
    # cv.imshow('test', result)
    # TO 此处的try希望能够在没有检测到圆的时候不让程序崩溃，并且希望能返回一个值用于目标是否存在的检测
    # try:
    #     circles = cv.HoughCircles(resultball, cv.HOUGH_GRADIENT, 1, 60, param1=1, param2=5, minRadius=10, maxRadius=50)
    #     # 参数4是圆心距离，param2是v2.HOUGH_GRADIENT方法的累加器阈值。阈值越小，检测到的圈子越多。
    #     # print circles
    #     for circle in circles[0]:
    #         x = int(circle[0])
    #         y = int(circle[1])
    #         r = int(circle[2])
    #         Imag1 = cv.circle(Image, (x, y), r, (0, 255, 0), 1)
    #         result = cv.circle(Imag1, (x, y), 2, (0, 255, 0), -1)
    #     databall = [x, y]
    # except TypeError:
    #     print 'NULL NULL NULL NULL NULL NULL NULL'
    try:
        x, y, w, h = cv.boundingRect(resulthole)
        cv.rectangle(Image, (x, y), (x + w, y + h), (0, 255, 0), 2)
        result = Image
        # datahole = [x + w / 2, y + h]
    except TypeError:
        print 'NULL NULL NULL NULL NULL NULL NULL'
    # print data
    cv.imshow('test', result)
    try:
        data = [x, y, w, h]
        return data
    except UnboundLocalError:
        print 'NULL NULL NULL NULL NULL NULL NULL'
        return [0, 0, 0, 0]


def getImag(IP, PORT, cameraID, flag, name=str(np.random.randint(0, 1000))):
    """
    获取图片，将10次的结果取平均，获得障碍物的坐标信息
    @param IP: 机器人IP
    @param PORT: 9559
    @param cameraID: 0为上摄像头，1为下摄像头
    @param flag: 为待处理的色彩，为字符串，"blue""red""yellow""white"
    @param name: 随机数生成的名称
    @return: 返回检测物的x，y，w，h
    """
    camProxy = ALProxy("ALVideoDevice", IP, PORT)
    resolution = vision_definitions.kQVGA
    colorSpace = vision_definitions.kBGRColorSpace
    fps = 20
    print "Is camera opened ?", camProxy.isCameraOpen(1)
    print "getting images in remote"
    nameID = camProxy.subscribe(str(name), resolution, colorSpace, fps)
    camProxy.setActiveCamera(cameraID)
    data = [0, 0, 0, 0]
    i = 0
    while i != 10:
        img = camProxy.getImageRemote(nameID)
        imagHeader0 = np.array(img[6])
        imagHeader = map(ord, img[6])
        camProxy.releaseImage(nameID)
        imagHeader = np.reshape(imagHeader, [240, 320, 3])
        img = np.uint8(imagHeader)
        # cv.imshow(str(name), img)
        data1 = ImagProgressHSV(img, flag)
        # if data[0] == data[1] == data[2] == data[3] == 0:
        #     continue
        data[0] += data1[0]
        data[1] += data1[1]
        data[2] += data1[2]
        data[3] += data1[3]
        i += 1
    data[0] /= 10
    data[1] /= 10
    data[2] /= 10
    data[3] /= 10
    cv.waitKey(1)
    print data
    return data


def getImagTEST(IP, PORT, cameraID, name):
    """
    得到图片一张

    :param IP: 机器人IP
    :param PORT: 9559
    :param cameraID: 0为上摄像头，1为下摄像头
    :param name: 相机名称，一般由随机数生成
    :return: 图片一张
    """
    camProxy = ALProxy("ALVideoDevice", IP, PORT)
    resolution = vision_definitions.kQVGA
    colorSpace = vision_definitions.kBGRColorSpace
    fps = 20
    print "Is camera opened ?", camProxy.isCameraOpen(1)
    print "getting images in remote"
    nameID = camProxy.subscribe(str(name), resolution, colorSpace, fps)
    camProxy.setActiveCamera(cameraID)
    # data = [90, 0, 0, 0]
    i = 0
    while True:
        img = camProxy.getImageRemote(nameID)
        imagHeader0 = np.array(img[6])
        imagHeader = map(ord, img[6])
        camProxy.releaseImage(nameID)
        imagHeader = np.reshape(imagHeader, [240, 320, 3])
        img = np.uint8(imagHeader)
        # cv.imshow(str(name), img)
        data1 = ImagProgressHSV(img)
        print data1
        print data1[2] * data1[3]
        cv.waitKey(1)


if __name__ == "__main__":
    robotIP = "169.254.75.185"
    PORT = 9559
    motionProxy = ALProxy("ALMotion", robotIP, PORT)
    postureProxy = ALProxy("ALRobotPosture", robotIP, PORT)
    motionProxy.wakeUp()
    postureProxy.goToPosture("StandInit", 0.5)
    name = str(random.randint(1, 1000))
    name = str(np.random.randint(0, 1000))
    print getImagTEST(robotIP, 9559, 1, name)
    cv.waitKey(1)
